#include "wrflash.h"
#include "IAP.h"
#include "string.h"

#define FLASH_SIZE_INSIDE   0x80000
#define FLASH_SIZE_OUTSIDE  0x800000
#define FLASH_SECTOR_NUM_INSIDE 128

uint32_t Address = 0;
__align(4) static uint8_t flashBufferData0[4096];
__align(4) static uint8_t flashBufferData1[4096];
__align(4) static uint8_t flashBufferData2[4096];
__align(4) static uint8_t flashBufferData3[4096];
__align(4) static uint8_t flashBufferData4[4096];
__align(4) static uint8_t flashBufferData5[4096];
__align(4) static uint8_t flashBufferData6[4096];
__align(4) static uint8_t flashBufferData7[4096];

uint8_t *flashBufferData[8] = {flashBufferData0, flashBufferData1, flashBufferData2, flashBufferData3, flashBufferData4,
                               flashBufferData5, flashBufferData6, flashBufferData7};

uint32_t CurrentSector = 0;
uint8_t WriteFlag = 0;
uint32_t currentSectorFirstAddress;
uint32_t nextSectorFirstAddress;


void SetSector();
uint8_t WriteFalsh();
void ReadFlash();
void SetStartAddress();
uint32_t CpyToRAM(uint32_t Index, uint8_t* pDataSource, uint32_t Length);
uint32_t CpyRAMTo(uint32_t Index, uint8_t* pDataTarget, uint32_t Length);

uint32_t CpyToRAM(uint32_t Index, uint8_t* pDataSource, uint32_t Length)
{
  uint8_t n = 0;
  uint32_t i = 0;
  for(i = 0; i < Length; i++)
  {
    n = (Index + i) / 4096;
    flashBufferData[n][(Index+i) - n*4096] = pDataSource[i];
  }
  return Length;
}

uint32_t CpyRAMTo(uint32_t Index, uint8_t* pDataTarget, uint32_t Length)
{
  uint8_t n = 0;
  uint32_t i = 0;
  for(i = 0; i < Length; i++)
  {
    n = (Index + i) / 4096;
    pDataTarget[i] = flashBufferData[n][(Index+i) - n*4096];
  }
  return Length;
}


void SetStartAddress()
{
  if(CurrentSector < 0x10)
  {
    currentSectorFirstAddress = CurrentSector * 4096;
    nextSectorFirstAddress = (CurrentSector + 1) * 4096;
  }
  else
  {
    currentSectorFirstAddress = (CurrentSector - 0x10) * 0x8000 + 0x10000;
    nextSectorFirstAddress = (CurrentSector - 0x10 + 1) * 0x8000 + 0x10000;
  }
}

uint8_t SetFlashAddress(uint32_t address)
{
  uint8_t err = 0;
  uint16_t i = 0;

  if(address <  0x3000)
    return 3;
  if(CurrentSector==0)
  {
    Address = address;
    SetSector();
    ReadFlash();
    return 0;
  }
  else if(address < currentSectorFirstAddress || address >= nextSectorFirstAddress)
  {
    err = WriteFalsh();
    if(err)
    {
      return err;
    }
    Address = address;
    SetSector();
    ReadFlash();
    return 0;
  }
  Address = address;
  SetSector();
  return 0;
}

uint8_t GetDataFromFlash(uint32_t Length, uint8_t *p)
{
  uint32_t startAddress = Address;
  uint16_t i = 0;

  if(Length + Address < nextSectorFirstAddress)
  {
    CpyRAMTo(Address - currentSectorFirstAddress, p, Length);
  }
  else
  {
    CpyRAMTo(Address - currentSectorFirstAddress, p, nextSectorFirstAddress - Address);
    startAddress = Address;
    Address = nextSectorFirstAddress;
    SetSector();
    ReadFlash();
    CpyRAMTo(0, p,Length - (nextSectorFirstAddress - startAddress));
  }
  Address = startAddress + Length;
  SetSector();
  return 0;
}

void ReadFlash()
{
  uint32_t i = 0, ii = 0;
  if(CurrentSector < 0x10)
  {
    for(ii = 0; ii < 0x1000; ii++)
    {
      flashBufferData[0][ii] = *(uint8_t*)(CurrentSector*0x1000 + ii);
    }
  }
  else
  {
    for(i = 0; i< 4; i++)
    {
      for(ii = 0; ii < 0x1000; ii++)
      {
        flashBufferData[i][ii] = *(uint8_t*)(CurrentSector*0x1000 + i * 4096 + ii);
      }
    }
  }
}

uint8_t AddDataToWriteData(uint32_t Length, uint8_t *p)
{
  uint16_t i = 0;
  uint8_t err;
  uint32_t startAddress = Address;
  
  if(Length + Address >= nextSectorFirstAddress)
  {
    
    CpyToRAM(Address - currentSectorFirstAddress, p, nextSectorFirstAddress - Address);
    WriteFlag = 1;
    err = WriteFalsh();
    if(err)
      return err;
    Address = nextSectorFirstAddress;
    SetSector();
    ReadFlash();
    if(Length > (nextSectorFirstAddress - startAddress))
    {
      CpyToRAM(0, p + nextSectorFirstAddress - startAddress, Length - (nextSectorFirstAddress - startAddress));
      WriteFlag = 1;
    }
    Address = startAddress + Length;
    SetSector();
    return 0;
  }
  CpyToRAM(Address - currentSectorFirstAddress, p, Length);
  WriteFlag = 1;
  Address = Address + Length;
  SetSector();
  return 0;
}

uint8_t WriteFalsh()
{
  uint8_t err;
  __disable_irq();
  
  if(CurrentSector < 0x10)
  {
    if(!WriteFlag)
    {
      __enable_irq();
      return 0;
    }
    WriteFlag = 0;
    err = u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }
    
    err = u32IAP_EraseSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }
    
    err = u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }
    
    err = u32IAP_CopyRAMToFlash(CurrentSector * 4096, (uint32_t)flashBufferData0, 4096);
    if(err)
    {
      __enable_irq();
      return err;
    }
  }
  else
  {
    err = u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }
    
    err  = u32IAP_EraseSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }
    
    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }
    
    err  = u32IAP_CopyRAMToFlash(0x10000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData0,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  = u32IAP_CopyRAMToFlash(0x11000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData1,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  = u32IAP_CopyRAMToFlash(0x12000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData2,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  = u32IAP_CopyRAMToFlash(0x13000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData3,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  = u32IAP_CopyRAMToFlash(0x14000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData4,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  = u32IAP_CopyRAMToFlash(0x15000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData5,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  = u32IAP_CopyRAMToFlash(0x16000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData6,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  =  u32IAP_PrepareSectors(CurrentSector, CurrentSector);
    if(err)
    {
      __enable_irq();
      return err;
    }

    err  = u32IAP_CopyRAMToFlash(0x17000 + (CurrentSector-16)*0x8000, (uint32_t)flashBufferData7,  0x1000);
    if(err)
    {
      __enable_irq();
      return err;
    }
  }
  __enable_irq();
  return 0;
}

void SetSector()
{
  if(Address < 0x10000)
  {
    CurrentSector = Address / 4096;
  }
  else
  {
    CurrentSector = (Address - 0x10000) / 0x8000 + 0x10;
  }
  SetStartAddress();
}


/*void testSectorWrite()
{
  uint8_t data[256];
  uint16_t i, ii = 0;
  SetFlashAddress(0x3000);

  
  for(i=0; i < 256; i++)
  {    
    data[i] = i;
  }
  for(i = 0; i < 128; i++)
  {
    for(ii = 0; ii < 16; ii++)
    {
      AddDataToWriteData(256, data);
    }
  }

 }*/




